//*************************************************************************
void resetINU(void) {
  
  // Re-Calibrates the gyros and initializes the IMU
  gyro.set_offset(256);
  
  // Get Accelerometer & compass for initial attitude
  getCompass();
  getAccel();
  
  int pitch = FP_math::fp_atan2(accel.s.x, -accel.s.z);
  if (pitch > FP_math::D180) pitch -= FP_math::D360;
  
  int roll = FP_math::fp_atan2(-accel.s.y, -accel.s.z);
  if (roll > FP_math::D180) roll -= FP_math::D360;
  
  inu.attitude.setQuat(compass_head, pitch, roll);
  
  delay(5);
  
}

void getCompass(void) {
  compass_head = compass.heading() - delta_head;
  if (compass_head < 0) compass_head += FP_math::D360;
}

void getAccel(void) {
  
  bma180.get_accel();
  accel.s.x = bma180.y_accel;
  accel.s.x += accel.s.x >> AgXSHIFT;
  
  accel.s.y = bma180.x_accel;
  int yG = accel.s.y >> AgYSHIFT;
  accel.s.y += yG + (yG >> 1);
  
  accel.s.z = -bma180.z_accel;
  accel.s.z += accel.s.z >> AgZSHIFT;
}

void getGyro(void) {
  
  gyro.get_rates();
  gyroRate.s.x = -gyro.y_gyro;
  gyroRate.s.y = -gyro.x_gyro;
  gyroRate.s.z = -gyro.z_gyro;
  
}


void getGPSvel(void) {
  gpsVelocity.s.x = 0;
  gpsVelocity.s.y = 0;
  gpsVelocity.s.z = 0;
}


